#ifndef SOPHUS_SIM3_HPP
#define SOPHUS_SIM3_HPP

#include "rxso3.hpp"

namespace Sophus {
template <class Scalar_, int Options = 0>
class Sim3;
using Sim3d = Sim3<double>;
using Sim3f = Sim3<float>;
}

namespace Eigen {
namespace internal {

template <class Scalar_, int Options>
struct traits<Sophus::Sim3<Scalar_, Options>> {
  using Scalar = Scalar_;
  using TranslationType = Sophus::Vector3<Scalar>;
  using RxSO3Type = Sophus::RxSO3<Scalar>;
};

template <class Scalar_, int Options>
struct traits<Map<Sophus::Sim3<Scalar_>, Options>>
    : traits<Sophus::Sim3<Scalar_, Options>> {
  using Scalar = Scalar_;
  using TranslationType = Map<Sophus::Vector3<Scalar>, Options>;
  using RxSO3Type = Map<Sophus::RxSO3<Scalar>, Options>;
};

template <class Scalar_, int Options>
struct traits<Map<Sophus::Sim3<Scalar_> const, Options>>
    : traits<Sophus::Sim3<Scalar_, Options> const> {
  using Scalar = Scalar_;
  using TranslationType = Map<Sophus::Vector3<Scalar> const, Options>;
  using RxSO3Type = Map<Sophus::RxSO3<Scalar> const, Options>;
};
}  // namespace internal
}  // namespace Eigen

namespace Sophus {

// Sim3 base type - implements Sim3 class but is storage agnostic.
//
// Sim(3) is the group of rotations  and translation and scaling in 3d. It is
// the semi-direct product of R+xSO(3) and the 3d Euclidean vector space.  The
// class is represented using a composition of RxSO3  for scaling plus
// rotation and a 3-vector for translation.
//
// Sim(3) is neither compact, nor a commutative group.
//
// See RxSO3 for more details of the scaling + rotation representation in
// 3d.
//
template <class Derived>
class Sim3Base {
 public:
  using Scalar = typename Eigen::internal::traits<Derived>::Scalar;
  using TranslationType =
      typename Eigen::internal::traits<Derived>::TranslationType;
  using RxSO3Type = typename Eigen::internal::traits<Derived>::RxSO3Type;
  using QuaternionType = typename RxSO3Type::QuaternionType;

  // Degrees of freedom of manifold, number of dimensions in tangent space
  // (three for translation, three for rotation and one for scaling).
  static int constexpr DoF = 7;
  // Number of internal parameters used (4-tuple for quaternion, three for
  // translation).
  static int constexpr num_parameters = 7;
  // Group transformations are 4x4 matrices.
  static int constexpr N = 4;
  using Transformation = Matrix<Scalar, N, N>;
  using Point = Vector3<Scalar>;
  using Tangent = Vector<Scalar, DoF>;
  using Adjoint = Matrix<Scalar, DoF, DoF>;

  // Adjoint transformation
  //
  // This function return the adjoint transformation ``Ad`` of the group
  // element ``A`` such that for all ``x`` it holds that
  // ``hat(Ad_A * x) = A * hat(x) A^{-1}``. See hat-operator below.
  //
  SOPHUS_FUNC Adjoint Adj() const {
    Matrix3<Scalar> const R = rxso3().rotationMatrix();
    Adjoint res;
    res.setZero();
    res.block(0, 0, 3, 3) = scale() * R;
    res.block(0, 3, 3, 3) = SO3<Scalar>::hat(translation()) * R;
    res.block(0, 6, 3, 1) = -translation();
    res.block(3, 3, 3, 3) = R;
    res(6, 6) = 1;
    return res;
  }

  // Returns copy of instance casted to NewScalarType.
  //
  template <class NewScalarType>
  SOPHUS_FUNC Sim3<NewScalarType> cast() const {
    return Sim3<NewScalarType>(rxso3().template cast<NewScalarType>(),
                               translation().template cast<NewScalarType>());
  }

  // Returns group inverse.
  //
  SOPHUS_FUNC Sim3<Scalar> inverse() const {
    RxSO3<Scalar> invR = rxso3().inverse();
    return Sim3<Scalar>(invR, invR * (translation() * Scalar(-1)));
  }

  // Logarithmic map
  //
  // Returns tangent space representation of the instance.
  //
  SOPHUS_FUNC Tangent log() const { return log(*this); }

  // Returns 4x4 matrix representation of the instance.
  //
  // It has the following form:
  //
  //   | s*R t |
  //   |  o  1 |
  //
  // where ``R`` is a 3x3 rotation matrix, ``s`` a scale factor, ``t`` a
  // translation 3-vector and ``o`` a 3-column vector of zeros.
  //
  SOPHUS_FUNC Transformation matrix() const {
    Transformation homogenious_matrix;
    homogenious_matrix.template topLeftCorner<3, 4>() = matrix3x4();
    homogenious_matrix.row(3) =
        Matrix<Scalar, 4, 1>(Scalar(0), Scalar(0), Scalar(0), Scalar(1));
    return homogenious_matrix;
  }

  // Returns the significant first three rows of the matrix above.
  //
  SOPHUS_FUNC Matrix<Scalar, 3, 4> matrix3x4() const {
    Matrix<Scalar, 3, 4> matrix;
    matrix.template topLeftCorner<3, 3>() = rxso3().matrix();
    matrix.col(3) = translation();
    return matrix;
  }

  // Assignment operator.
  //
  template <class OtherDerived>
  SOPHUS_FUNC Sim3Base<Derived>& operator=(
      Sim3Base<OtherDerived> const& other) {
    rxso3() = other.rxso3();
    translation() = other.translation();
    return *this;
  }

  // Group multiplication, which is rotation plus scaling concatenation.
  //
  // Note: That scaling is calculated with saturation. See RxSO3 for
  // details.
  //
  SOPHUS_FUNC Sim3<Scalar> operator*(Sim3<Scalar> const& other) const {
    Sim3<Scalar> result(*this);
    result *= other;
    return result;
  }

  // Group action on 3-points.
  //
  // This function rotates, scales and translates a three dimensional point
  // ``p`` by the Sim(3) element ``(bar_sR_foo, t_bar)`` (= similarity
  // transformation):
  //
  //   ``p_bar = bar_sR_foo * p_foo + t_bar``.
  //
  SOPHUS_FUNC Point operator*(Point const& p) const {
    return rxso3() * p + translation();
  }

  // In-place group multiplication.
  //
  SOPHUS_FUNC Sim3Base<Derived>& operator*=(Sim3<Scalar> const& other) {
    translation() += (rxso3() * other.translation());
    rxso3() *= other.rxso3();
    return *this;
  }

  // Setter of non-zero quaternion.
  //
  // Precondition: ``quat`` must not be close to zero.
  //
  SOPHUS_FUNC void setQuaternion(Eigen::Quaternion<Scalar> const& quat) {
    rxso3().setQuaternion(quat);
  }

  // Accessor of quaternion.
  //
  SOPHUS_FUNC QuaternionType const& quaternion() const {
    return rxso3().quaternion();
  }

  // Returns Rotation matrix
  //
  SOPHUS_FUNC Matrix3<Scalar> rotationMatrix() const {
    return rxso3().rotationMatrix();
  }

  // Mutator of SO3 group.
  //
  SOPHUS_FUNC RxSO3Type& rxso3() {
    return static_cast<Derived*>(this)->rxso3();
  }

  // Accessor of SO3 group.
  //
  SOPHUS_FUNC RxSO3Type const& rxso3() const {
    return static_cast<Derived const*>(this)->rxso3();
  }

  // Returns scale.
  //
  SOPHUS_FUNC Scalar scale() const { return rxso3().scale(); }

  // Setter of quaternion using rotation matrix ``R``, leaves scale as is.
  //
  SOPHUS_FUNC void setRotationMatrix(Matrix3<Scalar>& R) {
    rxso3().setRotationMatrix(R);
  }

  // Sets scale and leaves rotation as is.
  //
  // Note: This function as a significant computational cost, since it has to
  // call the square root twice.
  //
  SOPHUS_FUNC void setScale(Scalar const& scale) { rxso3().setScale(scale); }

  // Setter of quaternion using scaled rotation matrix ``sR``.
  //
  // Precondition: The 3x3 matrix must be "scaled orthogonal"
  //               and have a positive determinant.
  //
  SOPHUS_FUNC void setScaledRotationMatrix(Matrix3<Scalar> const& sR) {
    rxso3().setScaledRotationMatrix(sR);
  }

  // Mutator of translation vector
  //
  SOPHUS_FUNC TranslationType& translation() {
    return static_cast<Derived*>(this)->translation();
  }

  // Accessor of translation vector
  //
  SOPHUS_FUNC TranslationType const& translation() const {
    return static_cast<Derived const*>(this)->translation();
  }

  ////////////////////////////////////////////////////////////////////////////
  // public static functions
  ////////////////////////////////////////////////////////////////////////////

  // Derivative of Lie bracket with respect to first element.
  //
  // This function returns ``D_a [a, b]`` with ``D_a`` being the
  // differential operator with respect to ``a``, ``[a, b]`` being the lie
  // bracket of the Lie algebra sim(3).
  // See ``lieBracket()`` below.
  //
  SOPHUS_FUNC static Adjoint d_lieBracketab_by_d_a(Tangent const& b) {
    Vector3<Scalar> const upsilon2 = b.template head<3>();
    Vector3<Scalar> const omega2 = b.template segment<3>(3);
    Scalar const sigma2 = b[6];

    Adjoint res;
    res.setZero();
    res.template topLeftCorner<3, 3>() =
        -SO3<Scalar>::hat(omega2) - sigma2 * Matrix3<Scalar>::Identity();
    res.template block<3, 3>(0, 3) = -SO3<Scalar>::hat(upsilon2);
    res.template topRightCorner<3, 1>() = upsilon2;
    res.template block<3, 3>(3, 3) = -SO3<Scalar>::hat(omega2);
    return res;
  }

  // Group exponential
  //
  // This functions takes in an element of tangent space and returns the
  // corresponding element of the group Sim(3).
  //
  // The first three components of ``a`` represent the translational part
  // ``upsilon`` in the tangent space of Sim(3), the following three components
  // of ``a`` represents the rotation vector ``omega`` and the final component
  // represents the logarithm of the scaling factor ``sigma``.
  // To be more specific, this function computes ``expmat(hat(a))`` with
  // ``expmat(.)`` being the matrix exponential and ``hat(.)`` the hat-operator
  // of Sim(3), see below.
  //
  SOPHUS_FUNC static Sim3<Scalar> exp(Tangent const& a) {
    // For the derivation of the exponential map of Sim(3) see
    // H. Strasdat, "Local Accuracy and Global Consistency for Efficient Visual
    // SLAM", PhD thesis, 2012.
    // http://hauke.strasdat.net/files/strasdat_thesis_2012.pdf (A.5, pp. 186)
    Vector3<Scalar> const upsilon = a.segment(0, 3);
    Vector3<Scalar> const omega = a.segment(3, 3);
    Scalar const sigma = a[6];
    Scalar theta;
    RxSO3<Scalar> rxso3 =
        RxSO3<Scalar>::expAndTheta(a.template tail<4>(), &theta);
    Matrix3<Scalar> const Omega = SO3<Scalar>::hat(omega);

    using std::abs;
    using std::sin;
    using std::cos;
    static Matrix3<Scalar> const I = Matrix3<Scalar>::Identity();
    static Scalar const one(1);
    static Scalar const half(0.5);
    Matrix3<Scalar> const Omega2 = Omega * Omega;
    Scalar const scale = rxso3.scale();
    Scalar A, B, C;
    if (abs(sigma) < Constants<Scalar>::epsilon()) {
      C = one;
      if (abs(theta) < Constants<Scalar>::epsilon()) {
        A = half;
        B = Scalar(1. / 6.);
      } else {
        Scalar theta_sq = theta * theta;
        A = (one - cos(theta)) / theta_sq;
        B = (theta - sin(theta)) / (theta_sq * theta);
      }
    } else {
      C = (scale - one) / sigma;
      if (abs(theta) < Constants<Scalar>::epsilon()) {
        Scalar sigma_sq = sigma * sigma;
        A = ((sigma - one) * scale + one) / sigma_sq;
        B = ((half * sigma * sigma - sigma + one) * scale) / (sigma_sq * sigma);
      } else {
        Scalar theta_sq = theta * theta;
        Scalar a = scale * sin(theta);
        Scalar b = scale * cos(theta);
        Scalar c = theta_sq + sigma * sigma;
        A = (a * sigma + (one - b) * theta) / (theta * c);
        B = (C - ((b - one) * sigma + a * theta) / (c)) * one / (theta_sq);
      }
    }
    Matrix3<Scalar> const W = A * Omega + B * Omega2 + C * I;
    return Sim3<Scalar>(rxso3, W * upsilon);
  }

  // Returns the ith infinitesimal generators of Sim(3).
  //
  // The infinitesimal generators of Sim(3) are:
  //
  //         |  0  0  0  1 |
  //   G_0 = |  0  0  0  0 |
  //         |  0  0  0  0 |
  //         |  0  0  0  0 |
  //
  //         |  0  0  0  0 |
  //   G_1 = |  0  0  0  1 |
  //         |  0  0  0  0 |
  //         |  0  0  0  0 |
  //
  //         |  0  0  0  0 |
  //   G_2 = |  0  0  0  0 |
  //         |  0  0  0  1 |
  //         |  0  0  0  0 |
  //
  //         |  0  0  0  0 |
  //   G_3 = |  0  0 -1  0 |
  //         |  0  1  0  0 |
  //         |  0  0  0  0 |
  //
  //         |  0  0  1  0 |
  //   G_4 = |  0  0  0  0 |
  //         | -1  0  0  0 |
  //         |  0  0  0  0 |
  //
  //         |  0 -1  0  0 |
  //   G_5 = |  1  0  0  0 |
  //         |  0  0  0  0 |
  //         |  0  0  0  0 |
  //
  //         |  1  0  0  0 |
  //   G_6 = |  0  1  0  0 |
  //         |  0  0  1  0 |
  //         |  0  0  0  0 |
  //
  // Precondition: ``i`` must be in [0, 6].
  //
  SOPHUS_FUNC static Transformation generator(int i) {
    SOPHUS_ENSURE(i >= 0 || i <= 6, "i should be in range [0,6].");
    Tangent e;
    e.setZero();
    e[i] = Scalar(1);
    return hat(e);
  }

  // hat-operator
  //
  // It takes in the 7-vector representation and returns the corresponding
  // matrix representation of Lie algebra element.
  //
  // Formally, the ``hat()`` operator of Sim(3) is defined as
  //
  //   ``hat(.): R^7 -> R^{4x4},  hat(a) = sum_i a_i * G_i``  (for i=0,...,6)
  //
  // with ``G_i`` being the ith infinitesimal generator of Sim(3).
  //
  SOPHUS_FUNC static Transformation hat(Tangent const& a) {
    Transformation Omega;
    Omega.template topLeftCorner<3, 3>() =
        RxSO3<Scalar>::hat(a.template tail<4>());
    Omega.col(3).template head<3>() = a.template head<3>();
    Omega.row(3).setZero();
    return Omega;
  }

  // Lie bracket
  //
  // It computes the Lie bracket of Sim(3). To be more specific, it computes
  //
  //   ``[omega_1, omega_2]_sim3 := vee([hat(omega_1), hat(omega_2)])``
  //
  // with ``[A,B] := AB-BA`` being the matrix commutator, ``hat(.) the
  // hat-operator and ``vee(.)`` the vee-operator of Sim(3).
  //
  SOPHUS_FUNC static Tangent lieBracket(Tangent const& a, Tangent const& b) {
    Vector3<Scalar> const upsilon1 = a.template head<3>();
    Vector3<Scalar> const upsilon2 = b.template head<3>();
    Vector3<Scalar> const omega1 = a.template segment<3>(3);
    Vector3<Scalar> const omega2 = b.template segment<3>(3);
    Scalar sigma1 = a[6];
    Scalar sigma2 = b[6];

    Tangent res;
    res.template head<3>() = SO3<Scalar>::hat(omega1) * upsilon2 +
                             SO3<Scalar>::hat(upsilon1) * omega2 +
                             sigma1 * upsilon2 - sigma2 * upsilon1;
    res.template segment<3>(3) = omega1.cross(omega2);
    res[6] = Scalar(0);

    return res;
  }

  // Logarithmic map
  //
  // Computes the logarithm, the inverse of the group exponential which maps
  // element of the group (rigid body transformations) to elements of the
  // tangent space (twist).
  //
  // To be specific, this function computes ``vee(logmat(.))`` with
  // ``logmat(.)`` being the matrix logarithm and ``vee(.)`` the vee-operator
  // of Sim(3).
  //
  SOPHUS_FUNC static Tangent log(Sim3<Scalar> const& other) {
    // The derivation of the closed-form Sim(3) logarithm for is done
    // analogously to the closed-form solution of the SE(3) logarithm, see
    // J. Gallier, D. Xu, "Computing exponentials of skew symmetric matrices and
    // logarithms of orthogonal matrices", IJRA 2002.
    // https://pdfs.semanticscholar.org/cfe3/e4b39de63c8cabd89bf3feff7f5449fc981d.pdf
    // (Sec. 6., pp. 8)
    Tangent res;
    Scalar theta;
    Vector4<Scalar> const omega_sigma =
        RxSO3<Scalar>::logAndTheta(other.rxso3(), &theta);
    Vector3<Scalar> const omega = omega_sigma.template head<3>();

    Scalar sigma = omega_sigma[3];

    using std::abs;
    using std::sin;
    using std::abs;
    static Matrix3<Scalar> const I = Matrix3<Scalar>::Identity();
    static Scalar const half(0.5);
    static Scalar const one(1);
    static Scalar const two(2);
    Matrix3<Scalar> const Omega = SO3<Scalar>::hat(omega);
    Matrix3<Scalar> const Omega2 = Omega * Omega;
    Scalar const scale = other.scale();
    Scalar const scale_sq = scale * scale;
    Scalar const theta_sq = theta * theta;
    Scalar const sin_theta = sin(theta);
    Scalar const cos_theta = cos(theta);

    Scalar a, b, c;
    if (abs(sigma * sigma) < Constants<Scalar>::epsilon()) {
      c = one - half * sigma;
      a = -half;
      if (abs(theta_sq) < Constants<Scalar>::epsilon()) {
        b = Scalar(1. / 12.);
      } else {
        b = (theta * sin_theta + two * cos_theta - two) /
            (two * theta_sq * (cos_theta - one));
      }
    } else {
      Scalar const scale_cu = scale_sq * scale;
      c = sigma / (scale - one);
      if (abs(theta_sq) < Constants<Scalar>::epsilon()) {
        a = (-sigma * scale + scale - one) / ((scale - one) * (scale - one));
        b = (scale_sq * sigma - two * scale_sq + scale * sigma + two * scale) /
            (two * scale_cu - Scalar(6) * scale_sq + Scalar(6) * scale - two);
      } else {
        Scalar const s_sin_theta = scale * sin_theta;
        Scalar const s_cos_theta = scale * cos_theta;
        a = (theta * s_cos_theta - theta - sigma * s_sin_theta) /
            (theta * (scale_sq - two * s_cos_theta + one));
        b = -scale *
            (theta * s_sin_theta - theta * sin_theta + sigma * s_cos_theta -
             scale * sigma + sigma * cos_theta - sigma) /
            (theta_sq * (scale_cu - two * scale * s_cos_theta - scale_sq +
                         two * s_cos_theta + scale - one));
      }
    }
    Matrix3<Scalar> const W_inv = a * Omega + b * Omega2 + c * I;

    res.segment(0, 3) = W_inv * other.translation();
    res.segment(3, 3) = omega;
    res[6] = sigma;
    return res;
  }

  // vee-operator
  //
  // It takes the 4x4-matrix representation ``Omega`` and maps it to the
  // corresponding 7-vector representation of Lie algebra.
  //
  // This is the inverse of the hat-operator, see above.
  //
  // Precondition: ``Omega`` must have the following structure:
  //
  //                |  g -f  e  a |
  //                |  f  g -d  b |
  //                | -e  d  g  c |
  //                |  0  0  0  0 | .
  //
  SOPHUS_FUNC static Tangent vee(Transformation const& Omega) {
    SOPHUS_ENSURE(
        Omega.row(3).template lpNorm<1>() < Constants<Scalar>::epsilon(),
        "Omega: \n%", Omega);
    Tangent upsilon_omega_sigma;
    upsilon_omega_sigma.template head<3>() = Omega.col(3).template head<3>();
    upsilon_omega_sigma.template tail<4>() =
        RxSO3<Scalar>::vee(Omega.template topLeftCorner<3, 3>());
    return upsilon_omega_sigma;
  }
};

// Sim3 default type - Constructors and default storage for Sim3 Type.
template <class Scalar_, int Options>
class Sim3 : public Sim3Base<Sim3<Scalar_, Options>> {
  using Base = Sim3Base<Sim3<Scalar_, Options>>;

 public:
  using Scalar = Scalar_;
  using Transformation = typename Base::Transformation;
  using Point = typename Base::Point;
  using Tangent = typename Base::Tangent;
  using Adjoint = typename Base::Adjoint;

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW

  // Default constructor initialize similiraty transform to the identity.
  //
  SOPHUS_FUNC Sim3() : translation_(Vector3<Scalar>::Zero()) {}

  // Copy constructor
  //
  template <class OtherDerived>
  SOPHUS_FUNC Sim3(Sim3Base<OtherDerived> const& other)
      : rxso3_(other.rxso3()), translation_(other.translation()) {}

  // Constructor from RxSO3 and translation vector
  //
  template <class OtherDerived>
  SOPHUS_FUNC Sim3(RxSO3Base<OtherDerived> const& rxso3,
                   Point const& translation)
      : rxso3_(rxso3), translation_(translation) {}

  // Constructor from quaternion and translation vector.
  //
  // Precondition: quaternion must not be close to zero.
  //
  SOPHUS_FUNC Sim3(Eigen::Quaternion<Scalar> const& quaternion,
                   Point const& translation)
      : rxso3_(quaternion), translation_(translation) {}

  // Constructor from 4x4 matrix
  //
  // Precondition: Top-left 3x3 matrix needs to be "scaled-orthogonal" with
  //               positive determinant. The last row must be (0, 0, 0, 1).
  //
  SOPHUS_FUNC explicit Sim3(Matrix<Scalar, 4, 4> const& T)
      : rxso3_(T.template topLeftCorner<3, 3>()),
        translation_(T.template block<3, 1>(0, 3)) {}

  // This provides unsafe read/write access to internal data. Sim(3) is
  // represented by an Eigen::Quaternion (four parameters) and a 3-vector. When
  // using direct write access, the user needs to take care of that the
  // quaternion is not set close to zero.
  //
  SOPHUS_FUNC Scalar* data() {
    // rxso3_ and translation_ are laid out sequentially with no padding
    return rxso3_.data();
  }

  // Const version of data() above.
  //
  SOPHUS_FUNC Scalar const* data() const {
    // rxso3_ and translation_ are laid out sequentially with no padding
    return rxso3_.data();
  }

  // Accessor of RxSO3
  //
  SOPHUS_FUNC RxSO3<Scalar>& rxso3() { return rxso3_; }

  // Mutator of RxSO3
  //
  SOPHUS_FUNC RxSO3<Scalar> const& rxso3() const { return rxso3_; }

  // Mutator of translation vector
  //
  SOPHUS_FUNC Vector3<Scalar>& translation() { return translation_; }

  // Accessor of translation vector
  //
  SOPHUS_FUNC Vector3<Scalar> const& translation() const {
    return translation_;
  }

 protected:
  RxSO3<Scalar> rxso3_;
  Vector3<Scalar> translation_;
};

template <class Scalar, int Options = 0>
using Sim3Group[[deprecated]] = Sim3<Scalar, Options>;

}  // namespace Sophus

namespace Eigen {

// Specialization of Eigen::Map for ``Sim3``.
//
// Allows us to wrap Sim3 objects around POD array.
template <class Scalar_, int Options>
class Map<Sophus::Sim3<Scalar_>, Options>
    : public Sophus::Sim3Base<Map<Sophus::Sim3<Scalar_>, Options>> {
  using Base = Sophus::Sim3Base<Map<Sophus::Sim3<Scalar_>, Options>>;

 public:
  using Scalar = Scalar_;
  using Transformation = typename Base::Transformation;
  using Point = typename Base::Point;
  using Tangent = typename Base::Tangent;
  using Adjoint = typename Base::Adjoint;

  EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
  using Base::operator*=;
  using Base::operator*;

  SOPHUS_FUNC Map(Scalar* coeffs)
      : rxso3_(coeffs),
        translation_(coeffs + Sophus::RxSO3<Scalar>::num_parameters) {}

  // Mutator of RxSO3
  //
  SOPHUS_FUNC Map<Sophus::RxSO3<Scalar>, Options>& rxso3() { return rxso3_; }

  // Accessor of RxSO3
  //
  SOPHUS_FUNC Map<Sophus::RxSO3<Scalar>, Options> const& rxso3() const {
    return rxso3_;
  }

  // Mutator of translation vector
  //
  SOPHUS_FUNC Map<Sophus::Vector3<Scalar>, Options>& translation() {
    return translation_;
  }

  // Accessor of translation vector
  SOPHUS_FUNC Map<Sophus::Vector3<Scalar>, Options> const& translation() const {
    return translation_;
  }

 protected:
  Map<Sophus::RxSO3<Scalar>, Options> rxso3_;
  Map<Sophus::Vector3<Scalar>, Options> translation_;
};

// Specialization of Eigen::Map for ``Sim3 const``.
//
// Allows us to wrap RxSO3 objects around POD array.
template <class Scalar_, int Options>
class Map<Sophus::Sim3<Scalar_> const, Options>
    : public Sophus::Sim3Base<Map<Sophus::Sim3<Scalar_> const, Options>> {
  using Base = Sophus::Sim3Base<Map<Sophus::Sim3<Scalar_> const, Options>>;

 public:
  using Scalar = Scalar_;
  using Transformation = typename Base::Transformation;
  using Point = typename Base::Point;
  using Tangent = typename Base::Tangent;
  using Adjoint = typename Base::Adjoint;

  EIGEN_INHERIT_ASSIGNMENT_EQUAL_OPERATOR(Map)
  using Base::operator*=;
  using Base::operator*;

  SOPHUS_FUNC Map(Scalar const* coeffs)
      : rxso3_(coeffs),
        translation_(coeffs + Sophus::RxSO3<Scalar>::num_parameters) {}

  // Accessor of RxSO3
  //
  SOPHUS_FUNC Map<Sophus::RxSO3<Scalar> const, Options> const& rxso3() const {
    return rxso3_;
  }

  // Accessor of translation vector
  //
  SOPHUS_FUNC Map<Sophus::Vector3<Scalar> const, Options> const& translation()
      const {
    return translation_;
  }

 protected:
  Map<Sophus::RxSO3<Scalar> const, Options> const rxso3_;
  Map<Sophus::Vector3<Scalar> const, Options> const translation_;
};
}

#endif
